Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed the problem of static layer not restoring old map values for footprint #4824

Merged
merged 21 commits into from
Feb 4, 2025

Conversation

CihatAltiparmak
Copy link
Contributor

@CihatAltiparmak CihatAltiparmak commented Jan 4, 2025


Basic Info

Info Please fill out this column
Ticket(s) this addresses #4825
Primary OS tested on Ubuntu
Robotic platform tested on gazebo simulation of turtlebot
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

  • I have removed clearing the map_buffer just after The method processMap is run so we can utilize map_buffer to restore the region cleared before.
  • I have removed map_received_in_update_bounds_ because seems we really don't need to.
  • I have left to lock with mutex inside processMap method because now we are using lock in parts we use processMap

Description of documentation updates required from your changes

We have added some methods to Costmap2d. So we may need to update Costmap2D API documentation.


Future work that may be required in bullet points

  • We can add some methods to Costmap2d. For example, we can should pass the lambda function setConvexPolygonCost in order to implement dynamically how the cost is set.

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Copy link
Contributor

mergify bot commented Jan 4, 2025

@CihatAltiparmak, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I didn't carefully review this yet (Saturday afternoon over coffee!), but wouldn't it be possible for us to simply remove the setConvexPolygonCost from the internal static layer grid and apply it to the master grid update (master_grid.setConvexPolygonCost()))?

That would apply it more globally however which might not be wanted in some cases (though I find that generally hard to believe some layers would be set for that and others are not, practically speaking).

Alternatively, you could have setConvexPolygonCost split into 2 stages. Stage 1 is getting the cells that belong to the footprint

  // we assume the polygon is given in the global_frame...
  // we need to transform it to map coordinates
  std::vector<MapLocation> map_polygon;
  for (unsigned int i = 0; i < polygon.size(); ++i) {
    MapLocation loc;
    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
      // ("Polygon lies outside map bounds, so we can't fill it");
      return false;
    }
    map_polygon.push_back(loc);
  }

  std::vector<MapLocation> polygon_cells;

  // get the cells that fill the polygon
  convexFillCells(map_polygon, polygon_cells);

  // NEW: Now return this!

And return those. The second step would be to obtain their values in the loop, store them, and set the new values for FREE. After we do the master grid update, use the originally obtained values to repopulate the static layer. You'd basically just break the setConvexPolygonCost into 3 methods: (1) get the cells that belong to a convex shape, (2) sets its value for a fixed value (for free), and (3) sets its value to using a vector of input of values stored originally (to reset).

I think that's similar to what you did, but I think this would require alot less changes.

@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Jan 4, 2025

Thank you for give a feedback in a light speed.

wouldn't it be possible for us to simply remove the setConvexPolygonCost from the internal static layer grid and apply it to the master grid update (master_grid.setConvexPolygonCost()))?

Of course, I can. I just wondered your idea about whether adding new method would be useful for future. For instance, I wanna use setConvexCost in such a way that I can update with max value (updateWithMax). I got my answer.

That would apply it more globally however which might not be wanted in some cases (though I find that generally hard to believe some layers would be set for that and others are not, practically speaking).

I agree with you. I could not get any chance to take a look at other layers, however I can see where this issue comes from. (#4282 )

@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 4, 2025

Ah, that is pretty recent. Yeah I think doing the second thing I recommended on splitting up the functions would be the best move here. Then, parameterize if you want to repopulate (see comment in the issue ticket)

@CihatAltiparmak CihatAltiparmak force-pushed the fix/static_map branch 2 times, most recently from ba90d2d to 9896607 Compare January 10, 2025 21:46
@CihatAltiparmak CihatAltiparmak marked this pull request as ready for review January 10, 2025 22:38
@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Jan 10, 2025

Hi @SteveMacenski , If you get a chance, would you take a look at my modifications, I've tried to minimize diffs to ease your work. Btw, I can consider to add the test cases to increase code coverage if you want. Additionaly, I will take a look at the other layers.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please revert changes to the workflows of the node, I think it creates a number of regressions. I think you should only need to adjust the updateCosts portion of the implementation. I can see why you did this, wanting to restore from the previous iteration's stored values, but I think you can do this much more easily by just restoring a given iteration's costs at the end of the updateCosts method.

nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think what this currently does is clear the footprint on the master grid after update, but that would bypass the set updateWithXYZ() policies that users can set in order to set how they want updates to be performed (i.e. use max value or have it overwrite). This would lose information like having non-zero costs due to sensor measurements, even if removing the static map values, which wouldn't be a solution without introducing a regression.

This was a good thought / nice clean way to do this while minimizing code changes, but I think we need to do what we discussed previously unless there's another way around that issue. Break up setConvexPolygonCost into a few methods inside of costmap2d, use those methods here to obtain the cost values originally in the layer's internal costmap, clear the costmap, update the master grid, then revert the layer's internal costmap.

Maybe: Is there a way to apply the updateWithXYZ() policies for the following block? That would reduce the number of localized grid operations back to the same we do now (just each one has an extra couple of checks, which should be minimal in terms of computational cost)

 if (footprint_clearing_enabled_) {
    master_grid.setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
  }

I also don't understand why updateFootprint as a method was updated. I think the footprint must be transformed into the current frame before touch-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?

@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Jan 27, 2025

Firstly, my apologies for long delay due to my exams so on.

I think what this currently does is clear the footprint on the master grid after update, but that would bypass the set updateWithXYZ() policies that users can set in order to set how they want updates to be performed (i.e. use max value or have it overwrite). This would lose information like having non-zero costs due to sensor measurements, even if removing the static map values, which wouldn't be a solution without introducing a regression.

I have a question here. why do we have to update the areas with max value or overwrite after clearing the footprint area? Doesn't footprint_clearing_enabled lose its meaning here? But if you still haven't changed your idea, I will try to fix it in no time.

I also don't understand why updateFootprint as a method was updated. I think the footprint must be transformed into the current frame before touch-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?

I commented on one of your reviews, but I saw this later. You are right. Ignore my comments in that review.

I will figure out all of them.

@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 27, 2025

No need for apologies, life happens 😉

I have a question here. why do we have to update the areas with max value or overwrite after clearing the footprint area? Doesn't footprint_clearing_enabled lose its meaning here? But if you still haven't changed your idea, I will try to fix it in no time.

There's a setting use_max_ that users set for each layer about how to combine the various costmap layer plugins into a single master costmap that's used by the planning/control algorithms. They include updating with max value (i.e. from the existing master costmap, which may have been partially updated by other layers, is this cost greater than the existing one?) or override value (i.e. this value always takes priority, no matter what is already in the master costmap or inserted from other layers before this one).

The behavior that this PR currently implements is overriding of values. The master costmap is always cleared fully under footprint, rather than having the option to combine using the max value that could already exist in the costmap from another layer or a previous update. Imagine you had a laser scan layer below this one - the measurements from the laser scan would be deleted which represent real data being lost by overriding them. The obstacle layer that processes the laser scan has itself a clear under footprint parameter that would need to be also enabled to deal with that. There could be situations where you might not set clear under footprint for all layers -- so deleting the footprint's costs for the full master grid without consulting its combination policy is important.

The clear under footprint is specific to each layer, not to the master costmap itself. The combination policy that the user selected the layer to run at should be respected :-)

if the combination policy is 'override', then what you have is exactly the same as doing the footprint clearing on the local internal costmap to the layer. However, if the policy is 'use maximum', then its not the same. Before this PR, the cleared values under the footprint in the static layer would contribute 0 cost areas, so that maximum value already in from other layers would be retained. The current implementation would clear the values in the master to 0, overriding other layers which previously it just wouldn't update in those areas.

@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Jan 29, 2025

Hello, I tried to apply your suggestion. But I have some concerns. I found out that updateFootprint is only executed when clear costmap entirely service is called. This bring about the costmap is updated only when no plan is found. Of course, this depends on how behavior tree is implemented. I need to do some brain storming about it. It is hard to see this issue in Obstacle Layer. Because, obstacle layer takes into account the obstacles and updates bounds frequently.

I wonder your feedbacks. If I am in the right way, I will continue by adding restore_cleared_map_region parameter. Then I will create a PR about this changes in nav2 docs.

(From this comment #4824 (review))

I also don't understand why updateFootprint as a method was updated. I think the footprint must be transformed into the current frame before touch-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?

I thought a lot about whether updating bounds for footprint was good or bad. I also understand your concern. The unexpected behaviours may occur. Imagine that you don't use inflation layer. mppi may enter the occupied area. So I wanted to do brain-storming here through this channel.

Additionally, I used for (const auto & x : list) method for some parts. If you don't like them, I can revert it quickly.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Almost perfect, just a couple of architectural changes to make these more portable for other applications, but then good to go! Thanks for your time and effort prototyping this a few ways!

nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
Signed-off-by: CihatAltiparmak <[email protected]>
@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Feb 1, 2025

Hello, It took some time because I think your suggested names for the methods sounds awkward. In other methods, cell stands only for the locations of cell. So, I decided to use map_region word instead. Unfortunately, I couldn't find a way to keep setConvexPolygonCost method optimized. But if you want, I can revert it(setConvexPolygonCost), which gives rise to keep similar code blocks in the costmap2d class.

Secondly, I've created the PR about the parameter we added in this PR. You can find it here.

Finally, restore_outdated_map depends on footprint_clearing_enabled. Is it strictly necessary to check it in dynamic parameter updates?

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Small nitpicks, but otherwise this is perfect and ready!

nav2_costmap_2d/plugins/static_layer.cpp Outdated Show resolved Hide resolved
nav2_costmap_2d/src/costmap_2d.cpp Outdated Show resolved Hide resolved
@SteveMacenski
Copy link
Member

I couldn't find a way to keep setConvexPolygonCost method optimized. But if you want, I can revert it(setConvexPolygonCost), which gives rise to keep similar code blocks in the costmap2d class.

In what way? Is this significantly slower now?

Something you can do is in getMapRegionOccupiedByPolygon, you don't need the 2x loops. In the first loop where we populate map_polygon, you can directly populate the polygon_map_region if you simply getCost at that point.

Finally, restore_outdated_map depends on footprint_clearing_enabled. Is it strictly necessary to check it in dynamic parameter updates?

That might be good yes. Only set the dynamic param if footprint_clearing_enabled_ == true

@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Feb 3, 2025

In what way? Is this significantly slower now?

In the latest implementation, the vector consists of the pairs instead of only location. so this will increase the storage size of the vector. When we leave setConvexPolygonCost as it is, getMapRegionOccupiedByPolygon and setConvexPolygonCost will have similar code block. That's what i say. But, I did not measure it. It is just my hypothesis. I will measure it and will give feedback.

Something you can do is in getMapRegionOccupiedByPolygon, you don't need the 2x loops. In the first loop where we populate map_polygon, you can directly populate the polygon_map_region if you simply getCost at that point.

We cannot handle this in the first loop. It's because the first loop only handles the lines of the polygon. To do that, we need to modify convexFillCells. Maybe, I can modify PolygonOutlineCells class which is passed as action to bresenham2D method. On the other hand, I am trying to modify as less method as possible. What is your opinion here?

Btw, I will apply your feedbacks today.

@SteveMacenski
Copy link
Member

Ok great!

We cannot handle this in the first loop. It's because the first loop only handles the lines of the polygon. To do that, we need to modify convexFillCells. Maybe, I can modify PolygonOutlineCells class which is passed as action to bresenham2D method. On the other hand, I am trying to modify as less method as possible. What is your opinion here?

You are right, ignore that 😄

- changed the name of the new param
- fixed bug in the setConvexPolygonCost
- Used reserve method of the vector instances
- Added checks in dynamic parameter update.

Signed-off-by: CihatAltiparmak <[email protected]>
@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Feb 3, 2025

Hello,

I conducted a basic benchmark to measure the effect of our changes. The execution time of my setConvexPolygonCost approximatelly is 1.34 times slower than previous one.

The mean of the default implementation's execution time : ~20000ns
The mean of the new implementation's execution time : ~27000ns

You can take a look at how i conducted this benchmark if you want.

As you said, I added checks in dynamic parameter update. But I wanna say that setting restore_cleared_footprint as true will change the default behaviour of the software stack. We should either inform the users about this change or set restore_cleared_footprint as false in default.

You are right, ignore that 😄

I think let's not ignore that. You know the better one, but it can be opened some follow-up issues for that. I feel like the architecture side turned out little bit bad. Anyway.

Copy link

codecov bot commented Feb 4, 2025

Codecov Report

Attention: Patch coverage is 88.46154% with 3 lines in your changes missing coverage. Please review.

Files with missing lines Patch % Lines
nav2_costmap_2d/plugins/static_layer.cpp 72.72% 3 Missing ⚠️
Files with missing lines Coverage Δ
..._costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp 100.00% <100.00%> (ø)
...ostmap_2d/include/nav2_costmap_2d/static_layer.hpp 100.00% <ø> (ø)
nav2_costmap_2d/src/costmap_2d.cpp 88.00% <100.00%> (+0.44%) ⬆️
nav2_costmap_2d/plugins/static_layer.cpp 75.22% <72.72%> (+1.67%) ⬆️

... and 7 files with indirect coverage changes

@SteveMacenski
Copy link
Member

SteveMacenski commented Feb 4, 2025

But I wanna say that setting restore_cleared_footprint as true will change the default behaviour of the software stack

I agree, but this is a good thing. The migration guide is the right place for that https://docs.nav2.org/migration/Jazzy.html. You can add in an entry about the new parameter and default value

Signed-off-by: CihatAltiparmak <[email protected]>
@SteveMacenski
Copy link
Member

The execution time of my setConvexPolygonCost approximatelly is 1.34 times slower than previous one.

What if you reserve in polygon_map_region there as well? I'm guessing most of those are the vector resizings since std::vector initializes with size of 0 and each time it resizes, it doubles and copies things over. For 100, that would remove ~6-7 copies.

@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Feb 4, 2025

Seems it speeds up when reserved the vector. But the new rate is ~1.31. I had to wait to get rid of the spikes in the benchmark.

(I am also trying the 2^7 :) 🤷‍♂️ )

The mean of the new implementation's execution time : ~26500ns

EDIT: turtlebot occupies 125 cells and I tried 2^7. It is ~25700ns because of allocations.

@SteveMacenski
Copy link
Member

What are you measuring, the static layer update time or setConvexPolygonCost? We don't actually use setConvexPolygonCost in the static layer anymore (only in the obstacle layer now). It might be good to check where that time is coming from so we understand at least.

But, it might be worth keeping that version optimized if need be (if measuring setConvexPolygonCost).

Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
@CihatAltiparmak
Copy link
Contributor Author

CihatAltiparmak commented Feb 4, 2025

What are you measuring, the static layer update time or setConvexPolygonCost?

I measured the execution time of the setConvexPolygonCost. I have realized that I was benchmarking on balanced mode of the CPU. Instead, I benchmarked on performance mode of CPU. The benchamark results changed significantly.

The execution time of setConvexPolygonCost (default implementation): ~4000ns
The execution time of setConvexPolygonCost (my latest implementation): ~4350ns

Consequently, It increases the computational time by ~8%.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Set the footprint clearing enabled and restore cleared footprint in https://github.com/ros-navigation/navigation2/blob/main/nav2_system_tests/src/system/nav2_system_params.yaml#L196-L198 so that we can have this exercised in a system test, then I can merge!

@SteveMacenski SteveMacenski merged commit 088c423 into ros-navigation:main Feb 4, 2025
10 of 11 checks passed
masf7g pushed a commit to quasi-robotics/navigation2 that referenced this pull request Feb 5, 2025
…otprint (ros-navigation#4824)

* Fix

Signed-off-by: CihatAltiparmak <[email protected]>

* Fixed some bugs and removed the unnecessary variables from class

Signed-off-by: CihatAltiparmak <[email protected]>

* Keep the branch updated with main

Signed-off-by: CihatAltiparmak <[email protected]>

* Called off API changes in costmap, added "restore_outdated_footprint"

Signed-off-by: CihatAltiparmak <[email protected]>

* Added the documentation for the new methods in static layer

Signed-off-by: CihatAltiparmak <[email protected]>

* Make CI happy

Signed-off-by: CihatAltiparmak <[email protected]>

* Apply suggestions and minimize diffs

Signed-off-by: CihatAltiparmak <[email protected]>

* Fix ament_cpp_lint

Signed-off-by: CihatAltiparmak <[email protected]>

* Revert changes in the method updateFootprint

Signed-off-by: CihatAltiparmak <[email protected]>

* Used cached data to restore map and broken up setConvexPolygonCost into two methods

Signed-off-by: CihatAltiparmak <[email protected]>

* Rename newly added methods

Signed-off-by: CihatAltiparmak <[email protected]>

* Added restore_outdated_map parameter

Signed-off-by: CihatAltiparmak <[email protected]>

* Make CI happy

Signed-off-by: CihatAltiparmak <[email protected]>

* Changed the name of the newly added parameter

- changed the name of the new param
- fixed bug in the setConvexPolygonCost
- Used reserve method of the vector instances
- Added checks in dynamic parameter update.

Signed-off-by: CihatAltiparmak <[email protected]>

* Remove unnecessary log

Signed-off-by: CihatAltiparmak <[email protected]>

* Add new member to MapLocation

Signed-off-by: CihatAltiparmak <[email protected]>

* Remove unnecessary header

Signed-off-by: CihatAltiparmak <[email protected]>

* Set the parameters footprint_clearing_enabled and restore_cleared_footprint enable in nav2_system_tests

Signed-off-by: CihatAltiparmak <[email protected]>

---------

Signed-off-by: CihatAltiparmak <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

The regions which static layer clears for footprint always remain as FREE_SPACE
2 participants